﻿/*!
* @file ImageBackgroundSubtractorNormalizedVectorDistance.cpp
* @author 橋本敦史
* @date Last Change:2010/Jun/18.
*/
#include "ImageBackgroundSubtractorNormalizedVectorDistance.h"

using namespace std;

namespace mmpl{
	namespace image{

		/*!
		* @brief デフォルトコンストラクタ
		*/
		BackgroundSubtractorNormalizedVectorDistance::BackgroundSubtractorNormalizedVectorDistance(double threshold,unsigned int block_size):threshold(threshold),block_size(block_size),isBGset(false){
		}
		/*!
		* @brief 背景画像をセットすコンストラクタ
		*/
		BackgroundSubtractorNormalizedVectorDistance::BackgroundSubtractorNormalizedVectorDistance(const Image& bgimage,double threshold,unsigned int block_size):threshold(threshold),block_size(block_size),isBGset(false){
			setBGImage(bgimage);
		}


		/*!
		* @brief デストラクタ
		*/
		BackgroundSubtractorNormalizedVectorDistance::~BackgroundSubtractorNormalizedVectorDistance(){

		}
		/*!
		* @brief 背景差分を行う
		* @param img ソース画像
		* @param oimg 出力画像
		*/
		void BackgroundSubtractorNormalizedVectorDistance::apply(const Image& img, BGSubtractedImage* oimg){
			// oimgのサイズを確保
			int width=img.getWidth();
			int height=img.getHeight();
			if(width!=oimg->getWidth())oimg->setWidth(width);
			if(height!=oimg->getHeight())oimg->setHeight(height);
			if(oimg->getChannels()!=img.getChannels())oimg->setChannels(img.getChannels());


			Image gray;
			if(img.getChannels()==1){
				gray = img;
			}
			else{
				// グレースケール画像に変換する
				gray.setWidth(width);
				gray.setHeight(height);
				gray.setChannels(1);
				gray.setDepth(img.getDepth());
				cvCvtColor(img.getIplImage(),gray.getIplImage(),CV_BGR2GRAY);
			}

			MmplVector srcvec,bgvec;
			for(int x=0;x<width;x++){
				srcvec = getNormalizedVector(gray,x,0);
				bgvec = getNormalizedVector(bgimage,x,0);
				for(int y=0;y<height;y++){
					MmplVector _srcvec(srcvec),_bgvec(bgvec);
					_srcvec.regularize(srcvec.size());
					_bgvec.regularize(bgvec.size());
					/*			std::cout << _srcvec << std::endl;
					std::cout << _bgvec << std::endl;
					std::cout << _srcvec.meanSquareError(_bgvec) << std::endl;
					*/			if(threshold>_srcvec.meanSquareError(_bgvec)){
						oimg->setColor(x,y,oimg->getBGColor());
					}
					else{
						oimg->setColor(x,y,img.getColor(x,y));
					}
					slideNormalizedVector(gray,x,y,&srcvec);
					slideNormalizedVector(bgimage,x,y,&bgvec);
					//			srcvec = getNormalizedVector(gray,x,y);
					//			bgvec = getNormalizedVector(bgimage,x,y);
				}
			}
		}

		/**** Accessor ****/

		/*!
		* @brief block_size に値をセットするAccessor
		* @param block_size セットしたい値
		*/
		void BackgroundSubtractorNormalizedVectorDistance::setBlockSize(unsigned int block_size ){
			if(block_size<2){
				this->block_size = 2;
				return;
			}
			this->block_size = block_size;
		}

		/*!
		* @brief block_size の値を取得する
		* @param block_size block_sizeの値
		*/
		unsigned int BackgroundSubtractorNormalizedVectorDistance::getBlockSize()const{
			return block_size;
		}

		/*!
		* @brief threshold に値をセットするAccessor
		* @param threshold セットしたい値
		*/
		void BackgroundSubtractorNormalizedVectorDistance::setThreshold(double threshold ){
			this->threshold = threshold;
		}

		/*!
		* @brief threshold の値を取得する
		* @param threshold thresholdの値
		*/
		double BackgroundSubtractorNormalizedVectorDistance::getThreshold()const{
			return threshold;
		}

		/*!
		* @brief bgimage に値をセットするAccessor
		* @param bgimage セットしたい値
		*/
		void BackgroundSubtractorNormalizedVectorDistance::setBGImage(const Image& bgimage ){
			isBGset = true;
			if(bgimage.getChannels()==1){
				this->bgimage = bgimage;
				return;
			}

			// グレースケール画像に変換する
			if(this->bgimage.getWidth()!=bgimage.getWidth())
				this->bgimage.setWidth(bgimage.getWidth());
			if(this->bgimage.getHeight()!=bgimage.getHeight())
				this->bgimage.setHeight(bgimage.getHeight());
			if(this->bgimage.getChannels()!=1)
				this->bgimage.setChannels(1);
			if(this->bgimage.getDepth()!=bgimage.getDepth())
				this->bgimage.setDepth(bgimage.getDepth());

			cvCvtColor(bgimage.getIplImage(),this->bgimage.getIplImage(),CV_BGR2GRAY);
		}

		/*!
		* @brief bgimage の値を取得する
		* @param bgimage bgimageの値
		*/
		Image BackgroundSubtractorNormalizedVectorDistance::getBGImage()const{
			return bgimage;
		}


		/*!
		@brief 指定した場所で指定したサイズの輝度値ベクトルを作る
		@param src 元画像
		@param x (x,0)の回りでサイズblock_sizeのものを作る。但し画面の端の分はベクトルにしない
		@return 取得したベクトル
		*/
		MmplVector BackgroundSubtractorNormalizedVectorDistance::getNormalizedVector(const Image& src,int x,int y)const{
			MmplVector dst;
			int sx = x-block_size/2;
			int sy = y-block_size/2;
			if(sx<0) sx = 0;
			if(sy<0) sy = 0;
			int ex = x+block_size/2;
			int ey = y+block_size/2;
			if(ex>src.getWidth()) ex = src.getWidth();
			if(ey>src.getHeight()) ey = src.getHeight();
			dst.resize((ex-sx)*(ey-sy));
			int i=0;
			for(y = sy; y < ey; y++){
				for(x = sx; x < ex; x++,i++){
					dst[i] = static_cast<double>(src.getColor(x,y)[0]);
				}
			}
			return dst;
		}

		/*!
		@brief 前に取得したものをスライドさせてアクセスを減らす
		@param src 元画像
		@param x 次に取得するベクトルの中心x座標
		@param y 次に取得するベクトルの(中心y座標-1)に相当する値
		@param dst 返り値を格納
		*/
		void BackgroundSubtractorNormalizedVectorDistance::slideNormalizedVector(const Image& src,int x, int y,MmplVector* dst)const{
			int width;
			int height;
			int sx;
			width = block_size;

			//sx<0
			if(x-(static_cast<int>(block_size)/2)<0){
				// 十分な幅のwindowは確保できない
				width = x+(block_size/2);
				sx = 0;
			}
			else{
				width = block_size;
				sx = x-(block_size/2);
			}

			//ex>=src.getWidth()
			if(x+(static_cast<int>(block_size)/2)>=src.getWidth()){
				// 十分な幅のwindowは確保できない
				width = (block_size/2)+(src.getWidth()-x);
			}

			//sy<0
			if(y-(static_cast<int>(block_size/2))<0){
				height = y+(static_cast<int>(block_size/2));
			}
			else{
				// １行消去
				dst->erase(dst->begin(),dst->begin()+width);
				height = block_size;
			}


			// sy>=src.getHeight()
			if(y+(static_cast<int>(block_size)/2)>=src.getHeight()){
				// これ以上増やせない
				return;
			}

			dst->resize(width*height,0.0);
			y += (block_size/2);
			int i=width*(height-1);
			dst->resize(width*height);
			for(x=sx;x<sx+width;x++,i++){
				//		std::cerr << x << "," << y << std::endl;
				dst->at(i) = static_cast<double>(src.getColor(x,y)[0]);
			}
		}
	}
}
